
#include "main.h"

//typedef int                 int16;
//typedef long                int32;
//typedef long long           int64;
typedef unsigned int        Uint16;
typedef unsigned long       Uint32;
typedef unsigned long long  Uint64;
typedef float               float32;
typedef long double         float64;


#ifndef PI
    #define PI             3.1415926535897932384626433832795f   // CMSIS-DSP库 中有这个宏
#endif

#define OneThirdofPI   1.0471975511965977461542144610932f
#define ThirtyDividePI       9.5492965855137201461330258023509f
#define HUDUPI          6.283185307179586476925286766559f
#define PlusetoHUDU     0.000628318530717958647692528676f
#define  MinPulse         5000  //25us最小脉宽

void VoltageTest(void);
void SVPWM(void);

uint8_t test_stop_flag = 0;

float32 wt=0.0;
float32 nTime=0.0;
float32 Vmphase=0;
Uint16  CountVm=0;
Uint16  VmFlag=0;
float32 DeltaTs = 0.0008;
float32 FrebaseSet = 50;
float32 VpeakSet = 0.1;
uint32_t time1;
uint32_t time2;
uint32_t time_cost;
float32_t Me_ThetaOffset;



extern volatile int16_t g_speed_target;

#define VQS_SET_LIMIT (10.0f)
#define VDS_SET_LIMIT (10.0f)

void VoltageTest(void)
{
    wt = 2 * PI * FrebaseSet * CovModStrategy.nTime;    //这里nTime是指的是电机转动时间，而不是板卡运行时间
    while(wt>2*PI)
    {
        wt=wt-2*PI;
    }

    if(CovModStrategy.nTime < 5)//电机启动+匀速时间是1s
    {
        Vmphase = Vmphase + 0.001f;
        if(Vmphase >= VpeakSet)
        {
            Vmphase = VpeakSet;
        }
        test_stop_flag = 0;
    }
    else
    {
        Vmphase = Vmphase - 0.001f;
        if(Vmphase <= 0.001f)
        {
            test_stop_flag = 1;
            Vmphase = 0.001f;
            //这里加个标志位禁止使能
            StateSwitch(Machine_STOP);
            // DRV8353_cutDown();
        }

    }

    CovModStrategy.M_VAref= Vmphase*arm_cos_f32(wt);
    CovModStrategy.M_VBref= Vmphase*arm_cos_f32(wt-0.6666666667f*PI);
    CovModStrategy.M_VCref= Vmphase*arm_cos_f32(wt+0.6666666667f*PI);
}


float32 Current_wt=0.0;
float32 Current_Vmphase=0;
Uint16  Current_CountVm=0;
Uint16  Current_VmFlag=0;
float32 Current_FrebaseSet = 50;//基波频率设置
float32 Current_VpeakSet = 0.8;//电压幅值设置
float32 Current_ref_A = 0.0;
float32 Current_ref_B = 0.0;
float32 Current_ref_C = 0.0;
float32 Current_ref_Alpha = 0.0;
float32 Current_ref_Beta = 0.0;
float32 Current_ref_Id = 0.0;
float32 Current_ref_Iq = 0.0;

void AlgorithmInit(void)
{
    Current_wt=0.0f;
    Current_Vmphase=0;
    Current_CountVm=0;
    Current_VmFlag=0;
    Current_FrebaseSet = 50;//基波频率设置
    Current_VpeakSet = 0.8f;//电压幅值设置
    Current_ref_A = 0.0f;
    Current_ref_B = 0.0f;
    Current_ref_C = 0.0f;
    Current_ref_Alpha = 0.0f;
    Current_ref_Beta = 0.0f;
    Current_ref_Id = 0.0f;
    Current_ref_Iq = 0.0f;


    MCStrategy.Id_Set = 0;
    MCStrategy.Id_Feedback = 0;
    MCStrategy.Id_Error = 0;
    MCStrategy.Id_kp = 0.90f;////
    MCStrategy.Id_ki = 0.85f;////
    MCStrategy.Id_ki_Intergral = 0;
    MCStrategy.Id_Integrator_SYSTEM = 1;
    MCStrategy.Iq_Set = 0;
    MCStrategy.Iq_Feedback = 0;
    MCStrategy.Iq_Error = 0;
    MCStrategy.Iq_kp = 0.90f;////
    MCStrategy.Iq_ki = 0.85f;////
    MCStrategy.Iq_ki_Intergral = 0;
    MCStrategy.Iq_Integrator_SYSTEM = 1;
    MCStrategy.Vqs_Set = 0;
    MCStrategy.Vds_Set = 0;
    MCStrategy.wt = 0.0f;
    // MCStrategy.phir = 0.00386f;////
    MCStrategy.phir = 0.0386f;////
    MCStrategy.pn = 10;////
    MCStrategy.Lq = 0.80e-03;////   (mH)
    MCStrategy.Ld = 0.80e-03;////   (mH)
    MCStrategy.J = 0.398e-4;////
    MCStrategy.Ts = 1e-4;   /// 10Khz

    MCStrategy.Speed_kp = 0.0090f;
    MCStrategy.Speed_ki = 0.0055f;
    MCStrategy.Speed_kd = 0.0000f;
    // MCStrategy.Speed_kd = 0.00007f;

    MCStrategy.Speed_Set = 1470;     // 给定转速 (rpm)
    MCStrategy.Torque_Set = 0;
    MCStrategy.Speed_ki_Intergral = 0;
    MCStrategy.Torque_Limit_P = 3.5f;  // 扭矩限幅 Nm
    MCStrategy.Torque_Limit_N = -3.5f; // 扭矩限幅 Nm


    MCStrategy.Position_Set = 49.0f * (160.0f * PI / 180.0f);
    MCStrategy.Position_Feedback = 0.0f;
    MCStrategy.Position_Error = 0.0f;
    MCStrategy.Position_Error_last = 0.0f;
    MCStrategy.Position_kp = 40.0f;
    MCStrategy.Position_ki = 0.05f;
    MCStrategy.Position_ki_Intergral = 0.0f;
    MCStrategy.Position_kd = 0.0000f*0.0f;
    MCStrategy.Position_FilterCoefficient = 0.0f;
    MCStrategy.Position_Filter_DSTATE = 0.0f;

    MCStrategy.wt_qep = 0;
    MCStrategy.wt_qep_last = 0;
    MCStrategy.wt_delta = 0;
    MCStrategy.wm = 0;
    MCStrategy.Speed_Set_Ramp = 0;
    MCStrategy.wt_qep_sum = 0.0f;
    // MCStrategy.Speed_WmEnc = 150;
    // MCStrategy.Speed_Enc = 0.0006283185;  // 1
    MCStrategy.Speed_Enc = 0.003067961575771f;
    MCStrategy.TL = 0.1f;
    MCStrategy.Speed_WmEnc = 1500;
    MCStrategy.Speed_WmEnc_wz = fabs(MCStrategy.Speed_Set + 5) * 0.104719755119660;

    MCStrategy.Speed_Observer_h3 = MCStrategy.Speed_WmEnc_wz * MCStrategy.Speed_WmEnc_wz * MCStrategy.Speed_WmEnc_wz;
    MCStrategy.Speed_Observer_h2 = MCStrategy.Speed_WmEnc_wz * MCStrategy.Speed_WmEnc_wz * (1 + 2 * 0.707);
    MCStrategy.Speed_Observer_h1 = MCStrategy.Speed_WmEnc_wz * (1 + 2 * 0.707);
}

float stop_time = 0.0f;
void CurrentTest(void)
{
    Current_wt = 2*PI*Current_FrebaseSet*CovModStrategy.nTime;//这里nTime是指的是电机转动时间，而不是板卡运行时间
    if(Current_wt>2*PI)
    {
        Current_wt=Current_wt-2*PI;
    }

    if(CovModStrategy.nTime < 10)//电机启动+匀速时间是1s
    {
        Current_Vmphase=Current_Vmphase + 0.01f;
        if(Current_Vmphase>=Current_VpeakSet)
        {
            Current_Vmphase=Current_VpeakSet;
        }
        test_stop_flag = 0;

        // if (CovModStrategy.nTime >= 5 && CovModStrategy.nTime < 10)
        // {
        //     Current_FrebaseSet = 50;
        // }
        // else if (CovModStrategy.nTime >= 10 && CovModStrategy.nTime < 15)
        // {
        //     Current_FrebaseSet = 25;
        // }
    }
    else
    {
        Current_Vmphase=Current_Vmphase - 0.001f;
        if(Current_Vmphase<=0.001f)
        {
            test_stop_flag = 1;
            Current_Vmphase = 0.001f;
            //这里加个标志位禁止使能
            StateSwitch(Machine_STOP);
            stop_time = CovModStrategy.nTime;
        }
    }

    // Current_wt = PI * (60.0f / 180.0f);

    Current_ref_A = 10*Current_Vmphase*arm_cos_f32(Current_wt);//这里的10是控制电流幅值，10*0.8=8，电流幅值是8A。
    Current_ref_B = 10*Current_Vmphase*arm_cos_f32(Current_wt-0.6666666667f*PI);
    Current_ref_C = 10*Current_Vmphase*arm_cos_f32(Current_wt+0.6666666667f*PI);

    Current_ref_Alpha = (0.66666666666666666666666666666667f)*(Current_ref_A - 0.5f*(Current_ref_B + Current_ref_C));
    Current_ref_Beta =  (0.57735026918962576450914878050196f)*(Current_ref_B - Current_ref_C);

    Current_ref_Iq = Current_ref_Alpha * arm_cos_f32(Current_wt) + Current_ref_Beta * arm_sin_f32(Current_wt);
    Current_ref_Id = 0.0f;    //-1 * Current_ref_Alpha * sin(Current_wt) + Current_ref_Beta * cos(Current_wt);
    // Current_ref_Iq = 0.0f;
    // Current_ref_Id = 9.0f;    //-1 * Current_ref_Alpha * sin(Current_wt) + Current_ref_Beta * cos(Current_wt);
    MCStrategy.Iq_Set = Current_ref_Iq;
    MCStrategy.Id_Set = Current_ref_Id;

    MCStrategy.wt = Current_wt;
}

float min_val;
float max_val;
void find_min_max(float val)
{
    if (val < min_val) min_val = val;
    if (val > max_val) max_val = val;
}

void CurrentLoop(void)
{
    MCStrategy.CurrentA = g_invertorParam.motor_phaseCurrent.current_PhaseA;
    MCStrategy.CurrentB = g_invertorParam.motor_phaseCurrent.current_PhaseB;
    MCStrategy.CurrentC = g_invertorParam.motor_phaseCurrent.current_PhaseC;
    MCStrategy.CurrentAlpha = (0.66666666666666666666666666666667f)*(MCStrategy.CurrentA - 0.5f*(MCStrategy.CurrentB + MCStrategy.CurrentC));
    MCStrategy.CurrentBeta =  (0.57735026918962576450914878050196f)*(MCStrategy.CurrentB - MCStrategy.CurrentC);

    // CurrentTest();

    MCStrategy.Id_Feedback = MCStrategy.CurrentAlpha * arm_cos_f32(MCStrategy.wt) + MCStrategy.CurrentBeta * arm_sin_f32(MCStrategy.wt);
    MCStrategy.Iq_Feedback = -1 * MCStrategy.CurrentAlpha * arm_sin_f32(MCStrategy.wt) + MCStrategy.CurrentBeta * arm_cos_f32(MCStrategy.wt);

    //Id环
    MCStrategy.we = MCStrategy.pn * MCStrategy.wm;
    MCStrategy.Id_Set = 0;
    MCStrategy.Id_Error = MCStrategy.Id_Feedback - MCStrategy.Id_Set;
    if(MCStrategy.Id_Integrator_SYSTEM != 0)
    {
        MCStrategy.Id_ki_Intergral += MCStrategy.Ts * MCStrategy.Id_ki * MCStrategy.Id_Error;
    }
    else
    {
    }
    MCStrategy.Vds_Set = (MCStrategy.Id_kp * MCStrategy.Id_Error + MCStrategy.Id_ki_Intergral); // + (-1 * MCStrategy.we * MCStrategy.Lq * MCStrategy.Iq_Feedback);
    MCStrategy.Id_Integrator_SYSTEM = 0;
    MCStrategy.Id_ki_Intergral += MCStrategy.Ts * MCStrategy.Id_ki * MCStrategy.Id_Error;

    if(MCStrategy.Vds_Set > VDS_SET_LIMIT)
    {
    MCStrategy.Vds_Set = VDS_SET_LIMIT;
    }
    else if(MCStrategy.Vds_Set < -VDS_SET_LIMIT)
    {
        MCStrategy.Vds_Set = -VDS_SET_LIMIT;
    }

    //Iq环
    MCStrategy.Iq_Set = MCStrategy.Torque_Set * (2/(3*MCStrategy.pn*MCStrategy.phir));
    MCStrategy.Iq_Error = MCStrategy.Iq_Feedback - MCStrategy.Iq_Set;
    if(MCStrategy.Iq_Integrator_SYSTEM != 0)
    {
        MCStrategy.Iq_ki_Intergral += MCStrategy.Ts * MCStrategy.Iq_ki * MCStrategy.Iq_Error;
    }
    else
    {
    }
    MCStrategy.Vqs_Set = (MCStrategy.Iq_kp * MCStrategy.Iq_Error + MCStrategy.Iq_ki_Intergral); // + (MCStrategy.we * (MCStrategy.phir + MCStrategy.Ld * MCStrategy.Id_Feedback));
    MCStrategy.Iq_Integrator_SYSTEM = 0;

    // 积分限幅 防止积分饱和
    if (MCStrategy.Iq_ki_Intergral < -10.0f)
    {
        if (MCStrategy.Iq_Error > 0.0f)
        {
            MCStrategy.Iq_ki_Intergral += MCStrategy.Ts * MCStrategy.Iq_ki * MCStrategy.Iq_Error;
        }
        else
        {
            ;
        }
    }
    else if (MCStrategy.Iq_ki_Intergral > 10.0f)
    {
        if (MCStrategy.Iq_Error < 0.0f)
        {
            MCStrategy.Iq_ki_Intergral += MCStrategy.Ts * MCStrategy.Iq_ki * MCStrategy.Iq_Error;
        }
        else
        {
            ;
        }
    }
    else
    {
        MCStrategy.Iq_ki_Intergral += MCStrategy.Ts * MCStrategy.Iq_ki * MCStrategy.Iq_Error;
    }

    if(MCStrategy.Vqs_Set > VQS_SET_LIMIT)
    {
        MCStrategy.Vqs_Set = VQS_SET_LIMIT;
    }
    else if(MCStrategy.Vqs_Set < -VQS_SET_LIMIT)
    {
        MCStrategy.Vqs_Set = -VQS_SET_LIMIT;
    }
}

void angle_error(float ref, float fb, float* res)
{
    float d = ref - fb;
    // 用 atan2(sin,cos) 比 while 循环更数值稳健
    arm_atan2_f32(arm_sin_f32(d), arm_cos_f32(d), res);
}

float pos_Pout;
float pos_Iout;
float pos_Dout;

uint32_t time_position;
uint32_t lash_time_position;

uint8_t dir_flag = 0;   // 0 向上， 1 向下
int32_t swap_times = 0;
float32_t pos_dst_error = 0.0f;
void PositionLoop(void)
{

    // MCStrategy.Position_Feedback = MCStrategy.wt_qep;

    // time_position = (uint32_t)CovModStrategy.nTime;
    // if (time_position % 1 == 0 && time_position != lash_time_position)
    // {
    //     MCStrategy.Position_Set += 0.5233333f;
    //     if (MCStrategy.Position_Set > 2 * PI)
    //     {
    //         MCStrategy.Position_Set -= 2 * PI;
    //     }
    // }
    // lash_time_position = time_position;

    MCStrategy.Position_Feedback = MCStrategy.wt_qep_sum;

    // angle_error(MCStrategy.Position_Set, MCStrategy.Position_Feedback, &MCStrategy.Position_Error);
    MCStrategy.Position_Error = MCStrategy.Position_Set - MCStrategy.Position_Feedback;
   
    MCStrategy.Position_FilterCoefficient = ((MCStrategy.Position_kd * MCStrategy.Position_Error) - MCStrategy.Position_Filter_DSTATE) * 50;//滤波器，50为滤波系数N，同Simulink模块中的系数
    
    MCStrategy.Speed_Set = MCStrategy.Position_kp * MCStrategy.Position_Error + MCStrategy.Position_ki_Intergral + MCStrategy.Position_kd * (MCStrategy.Position_Error - MCStrategy.Position_Error_last) * MCStrategy.Ts;// + MCStrategy.Speed_FilterCoefficient;
    pos_Pout = MCStrategy.Position_kp * MCStrategy.Position_Error;
    pos_Iout = MCStrategy.Position_ki_Intergral;
    pos_Dout = MCStrategy.Position_kd * (MCStrategy.Position_Error - MCStrategy.Position_Error_last) * MCStrategy.Ts;

    if(MCStrategy.Speed_Set > 1200)
    {
        MCStrategy.Speed_Set = 1200;
    }
    else if(MCStrategy.Speed_Set < -1200)
    {
        MCStrategy.Speed_Set = -1200;
    }

    // if(MCStrategy.Position_ki_Intergral > 0.5)
    // {
    //     MCStrategy.Position_ki_Intergral = 0.5;
    // }
    // else if(MCStrategy.Position_ki_Intergral < -0.5)
    // {
    //     MCStrategy.Position_ki_Intergral = -0.5;
    // }

    MCStrategy.Position_ki_Intergral += MCStrategy.Position_ki * MCStrategy.Position_Error * MCStrategy.Ts;

    MCStrategy.Position_Filter_DSTATE += MCStrategy.Ts * MCStrategy.Position_FilterCoefficient;

    MCStrategy.Position_Error_last = MCStrategy.Position_Error;

    arm_abs_f32(&MCStrategy.Position_Error, &pos_dst_error, 1);
    if (pos_dst_error <= 1.1f )  /* 达到目标位置 */
    {
        if (dir_flag)
        {
            MCStrategy.Position_Set = 49.0f * (160.0f * PI / 180.0f);
            dir_flag = 0;
            swap_times ++;
        }
        else
        {
            MCStrategy.Position_Set = 49.0f * (-160.0f * PI / 180.0f);
            dir_flag = 1;
        }
    }

    if (swap_times >= 10)
    {
        StateSwitch(Machine_STOP);
    }
}


#if ABZ_ENCODER_SWITCH

int16_t QEP_Count_Sum = 0;
int16_t QEP_Count_delta = 0;

int16_t QEP_Count = 0;
int16_t QEP_Count_last = 0;

float speed_simple = 0.0f;
float speed_simlpe_rpm = 0.0f;



// 一阶低通滤波器结构体
typedef struct {
    float output;      // 滤波器输出
    float alpha;       // 滤波系数 (0~1)，越小滤波效果越强
} LowPassFilter;

LowPassFilter filter1 = 
{
    .output = 0.0f,
    .alpha = 0.0591173f
};


// 一阶低通滤波更新
float LowPassFilter_Update(LowPassFilter* filter, float input) {
    filter->output = filter->alpha * input + (1.0f - filter->alpha) * filter->output;
    return filter->output;
}

float actual_count = 0.0f;
float actual_speed = 0.0f;
float actual_theta_m = 0.0f;
float actual_theta_e = 0.0f;
float acc_w = 0.0f;
float observer_wt;
float observer_wm;
float test_show = 0.0f;
float test_show2 = 0.0f;
void SpeedObserver(void)
{
    QEP_Count = __HAL_TIM_GET_COUNTER(&htim5);
    actual_count = QEP_Count;

    QEP_Count_delta = QEP_Count_last - QEP_Count;
    QEP_Count_last = QEP_Count;

    if(QEP_Count_delta < -130)
    {
        QEP_Count_delta = QEP_Count_delta + 256;
    }
    else if(QEP_Count_delta > 130)
    {
        QEP_Count_delta = QEP_Count_delta - 256;
    }
    else
    {
        QEP_Count_delta = QEP_Count_delta;
    }

    QEP_Count_Sum += QEP_Count_delta;

    if(QEP_Count_Sum >= 2048)
    {
        QEP_Count_Sum -= 2048;
        MCStrategy.Speed_Observer_sum_interger -= (2 * PI);//6.28318530717958; 
    }
    else if(QEP_Count_Sum < -2048)
    {
        QEP_Count_Sum += 2048;
        MCStrategy.Speed_Observer_sum_interger += (2 * PI);//6.28318530717958; 
    }
    else
    {
        QEP_Count_Sum = QEP_Count_Sum;
    }

    MCStrategy.wt_qep = QEP_Count_Sum * 0.003067961575771f; // (2*pi/ENC) //0.000628318530717958

    MCStrategy.wt_delta = MCStrategy.wt_qep - MCStrategy.wt_qep_last;
    MCStrategy.wt_qep_last = MCStrategy.wt_qep;

    if(MCStrategy.wt_delta > PI)
    {
        MCStrategy.wt_delta -= 2 * PI;
    }
    else if(MCStrategy.wt_delta <  -1 * PI)
    {
        MCStrategy.wt_delta += 2 * PI;
    }
    else
    {
        MCStrategy.wt_delta = MCStrategy.wt_delta;
    }

    MCStrategy.wt_qep_sum += MCStrategy.wt_delta;   /* 多圈角度值 */


    // MCStrategy.Speed_Observer_h3_sum = ((MCStrategy.Torque_Set  - (MCStrategy.TL) / MCStrategy.J)) + MCStrategy.Speed_Observer_h3_interger;
    // MCStrategy.Speed_Observer_h3_sum = MCStrategy.Speed_Observer_h3_interger;
    MCStrategy.Speed_Observer_h3_sum = ((MCStrategy.Torque_Set /( MCStrategy.TL + MCStrategy.J))) + MCStrategy.Speed_Observer_h3_interger;

    if(MCStrategy.Speed_Observer_sum_interger_flag != 0)
    {
        MCStrategy.Speed_Observer_sum_interger_flag = MCStrategy.wt_qep;
    }

    MCStrategy.Speed_Observer_sum_interger_flag = 0;

    MCStrategy.Speed_Observer_input_sum = MCStrategy.wt_qep - (floor(MCStrategy.Speed_Observer_sum_interger / MCStrategy.Speed_Enc) * MCStrategy.Speed_Enc);
//     MCStrategy.Speed_Observer_input_sum = MCStrategy.wt_qep - MCStrategy.Speed_Observer_sum_interger;

    if(MCStrategy.Speed_Observer_input_sum > HUDUPI)
    {
        MCStrategy.Speed_Observer_input_sum -= HUDUPI;
    }
    else if(MCStrategy.Speed_Observer_input_sum < -1*HUDUPI)
    {
        MCStrategy.Speed_Observer_input_sum += HUDUPI;
    }
    else
    {
        MCStrategy.Speed_Observer_input_sum = MCStrategy.Speed_Observer_input_sum;
    }

    if((MCStrategy.Speed_Observer_input_sum > -1 * PlusetoHUDU) && (MCStrategy.Speed_Observer_input_sum < PlusetoHUDU))
    {
        if(MCStrategy.Speed_Observer_input_sum > 0.0f)
        {
            MCStrategy.Speed_Observer_input_sum = PlusetoHUDU;
        }
        else
        {
            MCStrategy.Speed_Observer_input_sum = 0.0f;
        }
    }

    MCStrategy.wm = MCStrategy.Speed_Observer_h2_interger;
    MCStrategy.wt = MCStrategy.Speed_Observer_sum_interger * MCStrategy.pn;

    MCStrategy.Speed_Observer_h3_interger += MCStrategy.Speed_Observer_h3 * MCStrategy.Speed_Observer_input_sum * MCStrategy.Ts;
    MCStrategy.Speed_Observer_sum_interger += (MCStrategy.Speed_Observer_h1 * MCStrategy.Speed_Observer_input_sum + MCStrategy.Speed_Observer_h2_interger) * MCStrategy.Ts;
    MCStrategy.Speed_Observer_h2_interger += (MCStrategy.Speed_Observer_h2 * MCStrategy.Speed_Observer_input_sum + MCStrategy.Speed_Observer_h3_sum) * MCStrategy.Ts;

    observer_wt = MCStrategy.wt;
    observer_wm = MCStrategy.wm;
}
#else

#define OFFSET_ANGLE    (2 * PI - Me_ThetaOffset)
void SpeedObserver(void)
{
    MCStrategy.wt_qep = tg_comm.encoder_theta + OFFSET_ANGLE;
    if(MCStrategy.wt_qep > 6.28f)   
    {
        MCStrategy.wt_qep -= 6.28f;
    }
    MCStrategy.wt_delta = MCStrategy.wt_qep - MCStrategy.wt_qep_last;
    MCStrategy.wt_qep_last = MCStrategy.wt_qep;

    if(MCStrategy.wt_delta > PI)
    {
        MCStrategy.wt_delta -= 2 * PI;
    }
    else if(MCStrategy.wt_delta <  -1 * PI)
    {
        MCStrategy.wt_delta += 2 * PI;
    }
    else
    {
        MCStrategy.wt_delta = MCStrategy.wt_delta;
    }

    MCStrategy.wm = (MCStrategy.wt_delta) / MCStrategy.Ts;

    MCStrategy.wt = MCStrategy.wt_qep * MCStrategy.pn;
}

#endif /* end of ABZ_ENCODER_SWITCH */



typedef struct {
    float32 state;          // 滤波器当前状态（平滑后的输出）
    float32 Ts;             // 控制周期（单位：秒）
    uint16_t N;             // 滤波系数（与Simulink一致）
} LowpassFilter;



LowpassFilter lowpassfilter = {
    .state = 0.0f,
    .Ts    = 2e-4f,
    .N     = 100
};
float32 LowpassFilter_Update(LowpassFilter *filter, float32 input) {
    
    float32 coefficient = (input - filter->state) * filter->N;
    filter->state += filter->Ts * coefficient;
    return filter->state;

}

float kp_out;
float ki_out;
float kd_out;
uint16_t normal_finished;
float32_t abs_error = 0.0f;
float32_t abs_set_speed = 0.0f;
void SpeedLoop(void)
{
   
    // if(CovModStrategy.nTime > 30)
    // {
    //     StateSwitch(Machine_STOP);
    //     normal_finished = 1;
    // }

    // //速度斜坡给定
    // if((MCStrategy.Speed_Set - MCStrategy.Speed_Set_Ramp) > MCStrategy.Speed_WmEnc * MCStrategy.Ts)//MCStrategy.Speed_WmEnc是转速变化率，原来是固定15000，现在用该变量替代
    // {
    //     MCStrategy.Speed_Set_Ramp += MCStrategy.Speed_WmEnc * MCStrategy.Ts;
    // }
    // else if((MCStrategy.Speed_Set - MCStrategy.Speed_Set_Ramp) < -1 * MCStrategy.Speed_WmEnc * MCStrategy.Ts)
    // {
    //     MCStrategy.Speed_Set_Ramp -= MCStrategy.Speed_WmEnc * MCStrategy.Ts;
    // }
    // else
    // {
    //     MCStrategy.Speed_Set_Ramp = MCStrategy.Speed_Set;
    // }

    // float Update_Speed =  ThirtyDividePI * MCStrategy.wm;
    // MCStrategy.Speed_Feedback = LowpassFilter_Update(&lowpassfilter,Update_Speed);     
    MCStrategy.Speed_Feedback = ThirtyDividePI * MCStrategy.wm; // rad/s -> rpm

    // MCStrategy.Speed_Error = MCStrategy.Speed_Set_Ramp - MCStrategy.Speed_Feedback;

    MCStrategy.Speed_Error = MCStrategy.Speed_Set - MCStrategy.Speed_Feedback;
    
    MCStrategy.Speed_FilterCoefficient = ((MCStrategy.Speed_kd * MCStrategy.Speed_Error) - MCStrategy.Speed_Filter_DSTATE) * 100;//滤波器，100为滤波系数N，同Simulink模块中的系数
    
    MCStrategy.Torque_Set = MCStrategy.Speed_kp * MCStrategy.Speed_Error + MCStrategy.Speed_ki_Intergral + MCStrategy.Speed_kd * (MCStrategy.Speed_Error - MCStrategy.Speed_Error_last) / MCStrategy.Ts;// + MCStrategy.Speed_FilterCoefficient;

    kp_out = MCStrategy.Speed_kp * MCStrategy.Speed_Error;
    ki_out = MCStrategy.Speed_ki_Intergral;
    kd_out =  MCStrategy.Speed_kd * (MCStrategy.Speed_Error - MCStrategy.Speed_Error_last) / MCStrategy.Ts;

    float ki_inc_step = MCStrategy.Speed_ki * MCStrategy.Speed_Error * MCStrategy.Ts;

     // 条件限幅
    if (MCStrategy.Speed_ki_Intergral >= 0.6f) 
    {
        // 积分项已经达到上限，只允许减小
        if (ki_inc_step < 0) 
        {
            MCStrategy.Speed_ki_Intergral += ki_inc_step;
        }

    } 
    else if (MCStrategy.Speed_ki_Intergral <= -0.6f) 
    {
        // 积分项已经达到下限，只允许增加
        if (ki_inc_step > 0) 
        {
            MCStrategy.Speed_ki_Intergral += ki_inc_step;
        }
    } 
    else
    {
        // 正常情况，直接累加
        MCStrategy.Speed_ki_Intergral += ki_inc_step;
    }
    
    MCStrategy.Speed_Filter_DSTATE += MCStrategy.Ts * MCStrategy.Speed_FilterCoefficient;

    MCStrategy.Speed_Error_last = MCStrategy.Speed_Error;

    


    if(MCStrategy.Torque_Set >= MCStrategy.Torque_Limit_P)
    {
        MCStrategy.Torque_Set = MCStrategy.Torque_Limit_P;
    }
    else if(MCStrategy.Torque_Set <= MCStrategy.Torque_Limit_N)
    {
        MCStrategy.Torque_Set = MCStrategy.Torque_Limit_N;
    }
    else
    {
        MCStrategy.Torque_Set = MCStrategy.Torque_Set;
    } 
}

void SVPWM(void)
{
    Uint16 A=0,B=0,C=0,N=0;
    float32 Vref_alpha=0,Vref_beta=0;
    // float32 Vref_d=0,Vref_q=0;
    float32 tempT = 0, X = 0, Y = 0, Z = 0;
    
    Vref_alpha = MCStrategy.Vds_Set * arm_cos_f32(MCStrategy.wt) - MCStrategy.Vqs_Set * arm_sin_f32(MCStrategy.wt);
    Vref_beta  = MCStrategy.Vds_Set * arm_sin_f32(MCStrategy.wt) + MCStrategy.Vqs_Set * arm_cos_f32(MCStrategy.wt);

    CovModStrategy.VAref = Vref_alpha;
    CovModStrategy.VBref = -0.5 * Vref_alpha + 0.86602540378443864676372317075294f * Vref_beta;
    CovModStrategy.VCref = -0.5 * Vref_alpha - 0.86602540378443864676372317075294f * Vref_beta;

    CovModStrategy.VdcLink = 48;

    CovModStrategy.M_VAref = CovModStrategy.VAref * (1.7320508075688772935274463415059f / CovModStrategy.VdcLink);//标幺Vdc/sqrt(3)
    CovModStrategy.M_VBref = CovModStrategy.VBref * (1.7320508075688772935274463415059f / CovModStrategy.VdcLink);
    CovModStrategy.M_VCref = CovModStrategy.VCref * (1.7320508075688772935274463415059f / CovModStrategy.VdcLink);

    // VoltageTest();

    CovModStrategy.Valpha = (0.66666666666666666666666666666667f)*(CovModStrategy.M_VAref - 0.5f*(CovModStrategy.M_VBref + CovModStrategy.M_VCref));
    CovModStrategy.Vbeta =  (0.57735026918962576450914878050196f)*(CovModStrategy.M_VBref - CovModStrategy.M_VCref);

    arm_sqrt_f32(CovModStrategy.Valpha * CovModStrategy.Valpha + CovModStrategy.Vbeta * CovModStrategy.Vbeta, &CovModStrategy.Vmag);

    if(CovModStrategy.Vmag>1)
    {
        CovModStrategy.Vmag=1;
    }

    if(CovModStrategy.Valpha==0)
    {
       if(CovModStrategy.Vbeta>0)
           CovModStrategy.Vang = 0.5f*PI;
       else if(CovModStrategy.Vbeta<0)
           CovModStrategy.Vang = 1.5f*PI;
       else 
           CovModStrategy.Vang = 0.0f;
    }
    else
    {
        arm_atan2_f32(CovModStrategy.Vbeta, CovModStrategy.Valpha, &CovModStrategy.Vang); 
        if(CovModStrategy.Vang < 0)
        {
            CovModStrategy.Vang = CovModStrategy.Vang + 2*PI;
        }
    }

    if(CovModStrategy.Vbeta >=0)
    {
        A = 1;
    }
    else
    {
        A = 0;
    }


    if((+1.732050808 * CovModStrategy.Valpha - CovModStrategy.Vbeta) >=0)
    {
        B = 1;
    }
    else
    {
        B = 0;
    }


    if((-1.732050808 * CovModStrategy.Valpha - CovModStrategy.Vbeta) >=0)
    {
        C = 1;
    }
    else
    {
        C = 0;
    }

    N = A + 2 * B + 4 * C;

    switch(N)
    {
    case 1:
        CovModStrategy.Sector =2;
        break;
    case 2:
        CovModStrategy.Sector =6;
        break;
    case 3:
        CovModStrategy.Sector =1;
        break;
    case 4:
        CovModStrategy.Sector =4;
        break;
    case 5:
        CovModStrategy.Sector =3;
        break;
    case 6:
        CovModStrategy.Sector =5;
        break;
    default:
        CovModStrategy.Sector =1;
        break;
    }

    
    X = CovModStrategy.Vmag * arm_sin_f32(CovModStrategy.Vang);
    Y = CovModStrategy.Vmag * arm_sin_f32(OneThirdofPI + CovModStrategy.Vang);
    Z = -1 * CovModStrategy.Vmag * arm_sin_f32(OneThirdofPI - CovModStrategy.Vang);
    
    switch(CovModStrategy.Sector)
    {
    case 1:
        CovModStrategy.Ta = -1 * Z;
        CovModStrategy.Tb = X;

        tempT = CovModStrategy.Ta + CovModStrategy.Tb;

        if(tempT > 1)
        {
            CovModStrategy.Ta = CovModStrategy.Ta / tempT;
            CovModStrategy.Tb = CovModStrategy.Tb / tempT;
        }

        CovModStrategy.Tc = 1 - CovModStrategy.Ta - CovModStrategy.Tb;

        CovModStrategy.Tao = 0.25f * CovModStrategy.Tc;
        CovModStrategy.Tbo = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta;
        CovModStrategy.Tco = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta + 0.5f * CovModStrategy.Tb;

        break;
    case 2:
        CovModStrategy.Ta = Z;
        CovModStrategy.Tb = Y;

        tempT = CovModStrategy.Ta + CovModStrategy.Tb;

        if(tempT > 1)
        {
            CovModStrategy.Ta = CovModStrategy.Ta / tempT;
            CovModStrategy.Tb = CovModStrategy.Tb / tempT;
        }

        CovModStrategy.Tc = 1 - CovModStrategy.Ta - CovModStrategy.Tb;
       
        CovModStrategy.Tao = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta;
        CovModStrategy.Tbo = 0.25f * CovModStrategy.Tc;
        CovModStrategy.Tco = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta + 0.5f * CovModStrategy.Tb;

        break;
    case 3:
        CovModStrategy.Ta = X;
        CovModStrategy.Tb = -1 * Y;
       
        tempT = CovModStrategy.Ta + CovModStrategy.Tb;

        if(tempT > 1)
        {
            CovModStrategy.Ta = CovModStrategy.Ta / tempT;
            CovModStrategy.Tb = CovModStrategy.Tb / tempT;
        }
            
        CovModStrategy.Tc = 1 - CovModStrategy.Ta - CovModStrategy.Tb;
        
        CovModStrategy.Tao = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta + 0.5f * CovModStrategy.Tb;
        CovModStrategy.Tbo = 0.25f * CovModStrategy.Tc;
        CovModStrategy.Tco = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta;

        break;
    case 4:
        CovModStrategy.Ta = -1 * X;
        CovModStrategy.Tb = Z;
        
        tempT = CovModStrategy.Ta + CovModStrategy.Tb;

        if(tempT > 1)
        {
            CovModStrategy.Ta = CovModStrategy.Ta / tempT;
            CovModStrategy.Tb = CovModStrategy.Tb / tempT;
        }
            
        CovModStrategy.Tc = 1 - CovModStrategy.Ta - CovModStrategy.Tb;
        
        CovModStrategy.Tao = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta + 0.5f * CovModStrategy.Tb;
        CovModStrategy.Tbo = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta;
        CovModStrategy.Tco = 0.25f * CovModStrategy.Tc;

        break;
    case 5:
        CovModStrategy.Ta = -1 * Y;
        CovModStrategy.Tb = -1 * Z;
        
        tempT = CovModStrategy.Ta + CovModStrategy.Tb;

        if(tempT > 1)
        {
            CovModStrategy.Ta = CovModStrategy.Ta / tempT;
            CovModStrategy.Tb = CovModStrategy.Tb / tempT;
        }
            
        CovModStrategy.Tc = 1 - CovModStrategy.Ta - CovModStrategy.Tb;
        
        CovModStrategy.Tao = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta;
        CovModStrategy.Tbo = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta + 0.5f * CovModStrategy.Tb;
        CovModStrategy.Tco = 0.25f * CovModStrategy.Tc;

        break;
    case 6:
        CovModStrategy.Ta = Y;
        CovModStrategy.Tb = -1 * X;
        
        tempT = CovModStrategy.Ta + CovModStrategy.Tb;

        if(tempT > 1)
        {
            CovModStrategy.Ta = CovModStrategy.Ta / tempT;
            CovModStrategy.Tb = CovModStrategy.Tb / tempT;
        }
            
        CovModStrategy.Tc = 1 - CovModStrategy.Ta - CovModStrategy.Tb;
       
        CovModStrategy.Tao = 0.25f * CovModStrategy.Tc;
        CovModStrategy.Tbo = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta + 0.5f * CovModStrategy.Tb;
        CovModStrategy.Tco = 0.25f * CovModStrategy.Tc + 0.5f * CovModStrategy.Ta;

        break;
    default:
        CovModStrategy.Ta = 0;
        CovModStrategy.Tb = 0;
        CovModStrategy.Tc = 0;
        CovModStrategy.Tao = 0;
        CovModStrategy.Tbo = 0;
        CovModStrategy.Tco = 0;
        break;
    }

    //******这里[0]是代表A相的切换计数值，[1]代表B相切换计数值，[2]代表C相切换计数值**********//
    // CovModStrategy.OutputSeq[0]=(Uint32)(CovModStrategy.Tao * CovModStrategy.TriangularCount);
    // CovModStrategy.OutputSeq[1]=(Uint32)(CovModStrategy.Tbo * CovModStrategy.TriangularCount);
    // CovModStrategy.OutputSeq[2]=(Uint32)(CovModStrategy.Tco * CovModStrategy.TriangularCount);

    CovModStrategy.TriangularCount = TIM8->ARR * 2;

    if (test_stop_flag)
    {
        CovModStrategy.Tao = 0;
        CovModStrategy.Tbo = 0;
        CovModStrategy.Tco = 0;	
    }

    __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_1, CovModStrategy.Tao * CovModStrategy.TriangularCount); //A相
    __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_2, CovModStrategy.Tbo * CovModStrategy.TriangularCount); //B相
    __HAL_TIM_SET_COMPARE(&htim8, TIM_CHANNEL_3, CovModStrategy.Tco * CovModStrategy.TriangularCount); //C相
}

//end of .c
